#include "elite_robot_sdk_protocol.h"

EliteRobotSdkProtocol::EliteRobotSdkProtocol()
{

}

void EliteRobotSdkProtocol::sdk_assemble_base_info(rapidjson::Writer<rapidjson::StringBuffer> &writer, std::string method_name, int robot_id)
{
    writer.Key("jsonrpc");
    writer.String("2.0");
    writer.Key("method");
    writer.String(method_name.c_str());
    writer.Key("id");
    writer.Int(robot_id);
}

std::string EliteRobotSdkProtocol::sdk_assemble_method_no_param(std::string method_name)
{
    rapidjson::StringBuffer strBuf;
    rapidjson::Writer<rapidjson::StringBuffer> writer(strBuf);

    writer.StartObject();
    sdk_assemble_base_info(writer, method_name.c_str(), ROBOT_ID_NUM);
    writer.EndObject();

    std::string cmd = strBuf.GetString();
    cmd +="\n";
    LOG(INFO) << "cmd= " << cmd << std::endl;
    return cmd;
}

std::string EliteRobotSdkProtocol::sdk_assemble_method_with_param(std::string method_name, EliteRobotSdkParamBase &param)
{
    rapidjson::StringBuffer strBuf;
    rapidjson::Writer<rapidjson::StringBuffer> writer(strBuf);

    writer.StartObject();
    sdk_assemble_base_info(writer, method_name.c_str(), ROBOT_ID_NUM);

    param.writer = &writer;
    param.set_params();
    writer.EndObject();

    std::string cmd = strBuf.GetString();
    cmd +="\n";

    LOG(INFO) << "cmd= " << cmd << "\n";
    return cmd;
}

int EliteRobotSdkProtocol::sdk_prase_bool_reply(std::string jsonStr, int &nReslut)
{
    rapidjson::Document dom;
    LOG(INFO) << "jsonStr= " << jsonStr<<"\n";
    if (dom.Parse(jsonStr.c_str()).HasParseError())
    {
        LOG(ERROR) << "sdk_prase_bool_reply JSON HasParse Error"<< std::endl;;
        return -ELITE_SDK_PARSE_JSON_STR_ERR;
    }

    if ( (!dom.HasMember("error")  || !dom["error"].IsObject()) &&
         (!dom.HasMember("result") || !dom["result"].IsString()) )
    {
        LOG(ERROR) << "sdk_prase_bool_reply JSON result Error"<< std::endl;
        return -ELITE_SDK_JSON_NO_RESLUT_STR_ERR;
    }

    if(dom.HasMember("result")){
        std::string resultStr = dom["result"].GetString();
        if(resultStr == "true"){
            nReslut =  ELITE_SDK_PARSE_TRUE;
        }else if(resultStr == "false"){
            nReslut =  ELITE_SDK_PARSE_FALSE;
        }else{
            LOG(ERROR) << "sdk_prase_bool_reply reslut str unknown resultStr =" << resultStr << std::endl;
            return -ELITE_SDK_JSON_RESLUT_STR_UNKNOWN_ERR;
        }
         LOG(INFO) << "resultStr= " << resultStr <<"; nReslut ="<< nReslut<<"\n";
    }else{
        LOG(INFO) << "json has no result set ELITE_SDK_PARSE_FALSE\n";
        nReslut = ELITE_SDK_PARSE_FALSE;
    }

    return ELITE_SDK_PARSE_JSON_OK;
}

int EliteRobotSdkProtocol::sdk_prase_int_reply(std::string jsonStr, int &nReslut)
{
    rapidjson::Document dom;
    LOG(INFO) << "jsonStr= " << jsonStr<<"\n";
    if (dom.Parse(jsonStr.c_str()).HasParseError())
    {
        LOG(ERROR) << "sdk_prase_bool_reply JSON HasParse Error"<< std::endl;;
        return -ELITE_SDK_PARSE_JSON_STR_ERR;
    }

    if (!dom.HasMember("result") || !dom["result"].IsString() )
    {
        LOG(ERROR) << "sdk_prase_bool_reply JSON result Error"<< std::endl;
        return -ELITE_SDK_JSON_NO_RESLUT_STR_ERR;
    }

    std::string resultStr = dom["result"].GetString();
    nReslut = std::stoi(resultStr);

    LOG(INFO) << "resultStr= " << resultStr<< "nReslut= " << nReslut<<"\n";
    return ELITE_SDK_PARSE_JSON_OK;
}

int EliteRobotSdkProtocol::sdk_prase_double_reply(std::string jsonStr, double &dReslut)
{
    rapidjson::Document dom;
    LOG(INFO) << "jsonStr= " << jsonStr<<"\n";
    if (dom.Parse(jsonStr.c_str()).HasParseError())
    {
        LOG(ERROR) << "sdk_prase_bool_reply JSON HasParse Error"<< std::endl;;
        return -ELITE_SDK_PARSE_JSON_STR_ERR;
    }

    if (!dom.HasMember("result") || !dom["result"].IsString() )
    {
        LOG(ERROR) << "sdk_prase_bool_reply JSON result Error"<< std::endl;
        return -ELITE_SDK_JSON_NO_RESLUT_STR_ERR;
    }

    std::string resultStr = dom["result"].GetString();
    dReslut = std::stod(resultStr);
    return ELITE_SDK_PARSE_JSON_OK;
}

int EliteRobotSdkProtocol::sdk_prase_6d_reply(std::string jsonStr, Param6D &param)
{

    rapidjson::Document dom;
    LOG(INFO) << "jsonStr= " << jsonStr<<"\n";
    if (dom.Parse(jsonStr.c_str()).HasParseError())
    {
        LOG(ERROR) << "sdk_prase_bool_reply JSON HasParse Error"<< std::endl;;
        return -ELITE_SDK_PARSE_JSON_STR_ERR;
    }

    if (!dom.HasMember("result") || !dom["result"].IsString() )
    {
        LOG(ERROR) << "sdk_prase_bool_reply JSON result Error"<< std::endl;
        return -ELITE_SDK_JSON_NO_RESLUT_STR_ERR;
    }

    std::string resultStr = dom["result"].GetString();
    std::vector<std::string> reslut = stringSplit(resultStr, ',');
    if(reslut.size()!=6){
        LOG(ERROR) << "sdk_prase_6d_reply reslut size Error!"<< "reslut.size()= "<<reslut.size()<< std::endl;
        return -ELITE_SDK_JSON_NO_RESLUT_STR_ERR;
    }
    for(int i=0;i<reslut.size();i++){
        param[i] = std::stod(reslut[i]);
    }

    return ELITE_SDK_PARSE_JSON_OK;
}

std::string EliteRobotSdkProtocol::sdk_assemble_getServoStatus_str()
{
    return sdk_assemble_method_no_param("getServoStatus");
}

std::string EliteRobotSdkProtocol::sdk_assemble_set_servo_status_str(EliteSetServoStatus servo_status)
{
    return sdk_assemble_method_with_param("set_servo_status", servo_status);
}

std::string EliteRobotSdkProtocol::sdk_assemble_syncMotorStatus_str()
{
    return sdk_assemble_method_no_param("syncMotorStatus");
}

std::string EliteRobotSdkProtocol::sdk_assemble_clearAlarm_str()
{
    return sdk_assemble_method_no_param("clearAlarm");
}

std::string EliteRobotSdkProtocol::sdk_assemble_getMotorStatus_str()
{
    return sdk_assemble_method_no_param("getMotorStatus");
}

std::string EliteRobotSdkProtocol::sdk_assemble_setCollisionEnable_str(EliteColEnable enable)
{
    return sdk_assemble_method_with_param("setCollisionEnable", enable);
}

std::string EliteRobotSdkProtocol::sdk_assemble_stop_str()
{
    return sdk_assemble_method_no_param("stop");
}

std::string EliteRobotSdkProtocol::sdk_assemble_runJbi_str(EliteJbiFilename jbi_filename)
{
    return sdk_assemble_method_with_param("runJbi", jbi_filename);
}

std::string EliteRobotSdkProtocol::sdk_assemble_setSpeed_str(EliteSpeedValue speed_value)
{
    return sdk_assemble_method_with_param("setSpeed", speed_value);
}

std::string EliteRobotSdkProtocol::sdk_assemble_getSysVarI_str(EliteGetSystemVar get_system_vari)
{
    return sdk_assemble_method_with_param("getSysVarI", get_system_vari);
}

std::string EliteRobotSdkProtocol::sdk_assemble_setSysVarI_str(EliteSetSystemVar<int> set_system_vari)
{
    return sdk_assemble_method_with_param("setSysVarI", set_system_vari);
}

std::string EliteRobotSdkProtocol::sdk_assemble_getSysVarD_str(EliteGetSystemVar get_system_vard)
{
    return sdk_assemble_method_with_param("getSysVarD", get_system_vard);
}

std::string EliteRobotSdkProtocol::sdk_assemble_setSysVarD_str(EliteSetSystemVar<double> get_system_vard)
{
    return sdk_assemble_method_with_param("setSysVarD", get_system_vard);
}
